(provide :rtmeus)
;;
(unless (boundp '*librtmeus-so*)
  (setq *librtmeus-so*
    (load-foreign
     (format nil "~A/RTMEus/~A/lib/librtmeus.so" (unix:getenv "RTMEUSDIR") (unix:getenv "ARCHDIR"))))
  )
;;
(defforeign init-rtm
  *librtmeus-so* "init_rtm" (:string :integer :integer) :integer)
(defforeign exit-rtm
  *librtmeus-so* "exit_rtm" nil nil)
(defforeign load-module
  *librtmeus-so* "load_module" (:integer :string :string) :integer)
(defforeign create-component
  *librtmeus-so* "create_component" (:integer :string :string :float :string :integer) :integer)
(defforeign resolve-component
  *librtmeus-so* "resolve_component" (:integer :string :string :integer) :integer)
(defforeign find-component
  *librtmeus-so* "find_component" (:integer :string :string) :integer)
(defforeign get-componentname
  *librtmeus-so* "get_componentname" (:integer :integer) (:string))
(defforeign activate-component
  *librtmeus-so* "activate_component" (:integer :integer) :integer)
(defforeign deactivate-component
  *librtmeus-so* "deactivate_component" (:integer :integer) :integer)
(defforeign set-configuration
  *librtmeus-so* "set_configuration" (:integer :integer :string :string) :integer)
(defforeign destroy
  *librtmeus-so* "destroy" (:integer :integer) :integer)
(defforeign show-component-info
  *librtmeus-so* "show_component_info" (:integer :integer :integer) :integer)
(defforeign inputdata-to-component
  *librtmeus-so* "inputdata_to_component" (:integer :integer :string :string :string) :integer)
(defforeign receivedata-from-component
  *librtmeus-so* "receivedata_from_component" (:integer :integer :string :string) (:string))
(defforeign sendcommand-to-component
  *librtmeus-so* "sendcommand_to_component" (:integer :integer :string :string) (:string))
(defforeign getstates-from-component
  *librtmeus-so* "getstates_from_component" (:integer :integer :string) (:string))
(defforeign setstates-to-component
  *librtmeus-so* "setstates_to_component" (:integer :integer :string :string) (:string))
(defforeign set-plugin-mode
  *librtmeus-so* "set_plugin_mode" (:integer :integer :integer :string) (:string))
(defforeign plugin-mode-allactivate
  *librtmeus-so* "plugin_mode_allactivate" (:integer :integer :integer :string :integer) (:string))
(defforeign plugin-mode-alldeactivate
  *librtmeus-so* "plugin_mode_alldeactivate" (:integer :integer :integer :string) (:string))
(defforeign connect-components
  *librtmeus-so* "connect_components" (:integer :integer :string :integer :integer :string :integer) :integer)
(defforeign disconnect-components
  *librtmeus-so* "disconnect_components" (:integer :integer :string :integer :integer :string) :integer)
;;
;;  this region should be removed after push-back is implemented on irteus
(defmacro push-back (el lst)
  `(if (null ,lst)
       (setf ,lst (list ,el))
     (nconc ,lst (list ,el))))
;;
(defclass RTComponent
  :super propertied-object
  :slots (rtm_no component_no)
  )
(defmethod RTComponent
  (:init
   (rno cno)
   (setq rtm_no rno
         component_no cno)
   self
   )
  (:activate nil (activate-component rtm_no component_no))
  (:deactivate nil (deactivate-component rtm_no component_no))
  (:name nil (get-componentname rtm_no component_no))
  (:set-configuration (confvar config) (set-configuration rtm_no component_no confvar config))
  (:set-conffile (conffile) (send self :set-configuration "componentconf_file" conffile))
  (:set-configname (cname)  (send self :set-configuration "config" cname))
  (:destroy nil (destroy rtm_no component_no))
  (:show-info (&optional (showlevel 0)) (show-component-info rtm_no component_no showlevel))  
  (:input-data
   (port data &key (datatype nil))
   (let ((strstrm (make-string-output-stream))
         senddata)
     ;; TimedDouble, TimedLong, TimedLongSeq, TimedDoubleSeq,etc is specified by key variable :datatype
     (if (not datatype)
         (cond
          ((and (vectorp data) (floatp (elt data 0)))
           (setq datatype "TimedFloatSeq"))
          ((and (vectorp data) (integerp (elt data 0)))
           (setq datatype "TimedShortSeq"))
          ((floatp data)
           (setq datatype "TimedFloat"))         
          ((integerp data)
           (setq datatype "TimedShort"))
          (t
           (warn "Plese specify datatype~%")
           (return-from :input-data nil)
           ))
       )
     ;;
     (cond
      ((or (equal datatype "TimedShort")
           (equal datatype "TimedLong"))
       (setq senddata (format nil "~A" data)))
      ((or (equal datatype "TimedDouble")
           (equal datatype "TimedFloat"))
       (setq senddata (format nil "~A" (float data))))
      ((or (equal datatype "TimedDoubleSeq")
           (equal datatype "TimedFloatSeq")
           (equal datatype "TimedLongSeq")
           (equal datatype "TimedShortSeq"))
       (format strstrm "[")
       (dotimes (i (- (length data) 1))
         (format strstrm "~A," (elt data i)))
       (format strstrm "~A]" (elt data (- (length data) 1)))
       (setq senddata (get-output-stream-string strstrm))
       )
      (t
       (warn "Unsupported datatype~%")
       (return-from :input-data nil)
       ))
     ;;
     (inputdata-to-component rtm_no component_no port datatype senddata)
     )
   )
  (:read-data 
   (port &key (datatype "TimedFloat"))
   ;;datatype is "TimedShort" or "TimedLong" or "TimedFloat" or "TimedDouble" or
   ;;            "TimedShortSeq" or "TimedLongSeq" or "TimedFloatSeq" or "TimedDoubleSeq"
   (read-from-string (receivedata-from-component rtm_no component_no port datatype))
   )
  (:command
   (command &optional (args ""))   
   (sendcommand-to-component rtm_no component_no (subseq (format nil "~A" command) 1) args)
   )
  (:get-state
   (&rest statenames)
   (let ((sendstr (make-string-output-stream))
         (nostr "")
         result)
     (dolist (sn statenames)
       (format sendstr "~A " (subseq (format nil "~A" sn) 1)))
     (setq result (getstates-from-component rtm_no component_no (string-right-trim " " (get-output-stream-string sendstr))))
     (if (string-equal result nostr)
         (print result)
         (read-from-string result))
     )
   )
  (:set-state
   (&rest statevaluelist) ;;(:state1 :float #f(1.0 1.4 2.0)) (:state2 :short #i(2 1 -1)) (:state3 :char #i(2 1 8)) ...
   (send self :set-states statevaluelist)
   )
  (:set-states
   (statevaluelist) ;;( (:state1 :float #f(1.0 1.4 2.0)) (:state2 :short #i(2 1 -1)) (:state3 :char #i(2 1 8)) ... )
   (let ((namestr (make-string-output-stream))
         (valuestr (make-string-output-stream))         
         (nostr "")
         name type value result)
     (dolist (sv statevaluelist)
       (setq name (elt sv 0)
             type (elt sv 1)
             value (elt sv 2))
       ;;
       (format namestr "~A " (subseq (format nil "~A" name) 1))
       ;;
       (case type
         (:char (format valuestr "c "))
         (:short (format valuestr "s "))
         (:long (format valuestr "l "))
         (:float (format valuestr "f "))
         (:double (format valuestr "d "))
         )
       ;;
       (dotimes (i (length value))
         (format valuestr "~A " (elt value i)))
       ;;
       (unless (= (position sv statevaluelist) (- (length statevaluelist) 1))
         (format valuestr ", "))
       )
     ;;
     (setq result (setstates-to-component
                   rtm_no component_no
                   (string-right-trim " " (get-output-stream-string namestr))
                   (string-right-trim " " (get-output-stream-string valuestr))))
     ;;
     (if (string-equal result nostr)
         (print result)
         (read-from-string result))
     )
   )
  (:get-rtm-no nil rtm_no)
  (:get-component-no nil component_no)
  )
;;
(defclass RTMeus
  :super propertied-object
  :slots (connection_no comps master-comp slave-comps plugin-mode)
  )

(defmethod RTMeus
  (:init
   (host &key (port 2809) (manno 0))
   (setq connection_no (init-rtm host port manno)
         comps nil
         master-comp nil
         slave-comps nil
         plugin-mode nil
         )
   self
   )
  (:exit () (exit-rtm))
  (:load-module
   (sopath initfuncname &key (rtmno connection_no))
   (load-module rtmno sopath initfuncname)
   )
  (:create-component
   (classname instancename &key (rtmno connection_no) (execrate 30.0) (exectype :periodic) (with-outstate t))
   (let (etype cno comp erate sflag)
     (setq erate (float execrate))
     (case exectype
       (:periodic
        (setq etype "PeriodicExecutionContext"))
       (:softrealtime-periodic
        (setq etype "SoftRealtimePeriodicExecutionContext"))
       (t
        (setq etype "PeriodicExecutionContext"))
       )
     (if with-outstate
         (setq sflag 1)
       (setq sflag 0))
     (setq cno (create-component rtmno classname instancename  erate etype sflag))
     (if (< cno 0) (return-from :create-component nil))
     (setq comp (instance RTComponent :init rtmno cno))
     (push-back comp comps)
     comp)
   )
  (:resolve-component
   (instancename &key (rtmno connection_no) (searchpath "/") (with-outstate nil))
   (let (cno comp sflag)
     (if with-outstate
         (setq sflag 1)
       (setq sflag 0))     
     (setq cno (resolve-component rtmno searchpath instancename sflag))
     (when (< cno 0);;already registerd
       (setq comp (send self :find-component-from-name instancename))
       (return-from :resolve-component comp))
     ;;
     (setq comp (instance RTComponent :init rtmno cno))
     (push-back comp comps)
     comp)
   )
  (:find-component-from-nameserver
   (instancename &key (rtmno connection_no) (searchpath "/"))
   (find-component rtmno searchpath instancename)
   )
  (:delete-component
   (tcomp)
   (send tcomp :destroy)
   (setq comps (remove tcomp comps))
   )
  (:find-component-from-name
   (cname)
   (let ((foundcmp_no (position cname comps :test #'(lambda (x y) (equal x (send y :name))))))
     (if foundcmp_no
         (elt comps foundcmp_no))
     )
   )
  (:connect-components
   (outcomp outport incomp inport &key (stype :flush))
   (let ((outcomp_no (send outcomp :get-component-no))
         (outrtmno (send outcomp :get-rtm-no))
         (incomp_no (send incomp :get-component-no))
         (inrtmno (send incomp :get-rtm-no))
	 subtype)
     (case 
      stype
      (:flush
       (setq subtype 0))
      (:new
       (setq subtype 1))
      (:periodic
       (setq subtype 2))
      (t
       (setq subtype 0))
      )
     (print outport)
     (print inport)
     ;;
     (connect-components outrtmno outcomp_no  outport  inrtmno incomp_no inport subtype)
     )
   )
  (:disconnect-components
   (outcomp outport incomp inport)
   (let ((outcomp_no (send outcomp :get-component-no))
         (outrtmno (send outcomp :get-rtm-no))
         (incomp_no (send incomp :get-component-no))
         (inrtmno (send incomp :get-rtm-no)))
     ;;
     (disconnect-components outrtmno outcomp_no  outport  inrtmno incomp_no inport)
     )   
   )
  (:set-master-comp
   (mcomp)
   (if (derivedp mcomp RTComponent)
       (setq master-comp mcomp)
       (warn "Not a RTComponent~%"))
   )
  (:add-slave-comp
   (acomp)
   (if (derivedp acomp RTComponent)
       (push-back acomp slave-comps)
       (warn "Not a RTComponent~%"))
   )
  (:set-plugin-mode
   (&key (rtmno connection_no) (remote nil))
   (when (or (not master-comp) (not slave-comps))
     (warn "register master & slave comps at first~%")
     (return-from :set-plugin-mode nil))
   (let ((lcomp_no (send master-comp :get-component-no))
         (acomp_no-vector (make-array (length slave-comps) :element-type integer-vector)))
     (when (not remote)
       (dotimes (i (length slave-comps))
         (setf (elt acomp_no-vector i) (send (elt slave-comps i) :get-component-no)))
       (set-plugin-mode rtmno lcomp_no (length acomp_no-vector) acomp_no-vector))
     ;;
     (setq plugin-mode t)
     )
   )
  (:plugin-mode-all-activate
   (&key (rtmno connection_no) (interval 1000))
   (when (or (not master-comp) (not slave-comps))
     (warn "register master & slave comps at first~%")
     (return-from :plugin-mode-all-activate nil))
   (let ((lcomp_no (send master-comp :get-component-no))
         (acomp_no-vector (make-array (length slave-comps) :element-type integer-vector)))
     (dotimes (i (length slave-comps))
       (setf (elt acomp_no-vector i) (send (elt slave-comps i) :get-component-no)))
     ;;
     (plugin-mode-allactivate rtmno lcomp_no (length acomp_no-vector) acomp_no-vector interval)
     )
   )
  (:plugin-mode-all-deactivate
   (&key (rtmno connection_no))
   (when (or (not master-comp) (not slave-comps))
     (warn "register master & slave comps at first~%")
     (return-from :plugin-mode-all-deactivate nil))
   (let ((lcomp_no (send master-comp :get-component-no))
         (acomp_no-vector (make-array (length slave-comps) :element-type integer-vector)))
     (dotimes (i (length slave-comps))
       (setf (elt acomp_no-vector i) (send (elt slave-comps i) :get-component-no)))
     ;;
     (plugin-mode-alldeactivate rtmno lcomp_no (length acomp_no-vector) acomp_no-vector)
     )
   )    
  (:comps nil comps)
  (:all-activate
   (&optional (wutime 1000))
   (if plugin-mode
       (send self :plugin-mode-all-activate :interval wutime)
       (dolist (c comps)
         (send c :activate)
         (unix:usleep wutime))
       )
   )
  (:all-deactivate
   ()
   (if plugin-mode
       (send self :plugin-mode-all-deactivate)
       (send-all comps :deactivate))
   )
  (:master-comp nil master-comp)
  (:slave-comps nil slave-comps)
  (:plugin-mode nil plugin-mode)
  )
;;
(setq sys::*exit-hook* 'exit-rtm)
;;
;;
(format t "do (setq *rtm* (instance RTMeus :init \"localhost\"))~%")
